#include "pid.h"
#include <stdio.h>
#include <stdlib.h>


pid_struct *pid_init(float P,float I,float D)
{
    pid_struct *p=(pid_struct *)calloc(1,sizeof(pid_struct));
    if(p==NULL)
    {
        return p; //分配内存失败，返回NULL
    }
    p->P=P;
    p->I=I;
    p->D=D;
    return p; 
}


int pid_init(pid_struct *pid,float P,float I,float D)
{
    if(pid==NULL)
    {
        return -1;
    }
    pid->P=P;
    pid->I=I;
    pid->D=D;
    return 0;
}


float pid_position_ctrl(float targetvalue,float feedback,pid_struct *pid)
{
    pid->error=targetvalue-feedback;
    pid->error_sum+=pid->error;
    pid->outvalue=pid->P*pid->error+pid->I*pid->error_sum+pid->D*(pid->error-pid->error_last);
    pid->error_last=pid->error;
    return pid->outvalue;
}

float pid_increment_ctrl(float targetvalue,float feedback,pid_struct *pid)
{
    pid->error=targetvalue-feedback;
    pid->outvalue+=pid->P*(pid->error-pid->error_last)+pid->I*pid->error+pid->D*(pid->error-2*pid->error_last+pid->error_last2);
    pid->error_last2=pid->error_last;
    pid->error_last=pid->error;
    return pid->outvalue;
}



